#include <iostream>
#include <fcntl.h>
#include <unistd.h>
#include <termios.h>
#include <cstring>
#include <vector>
#include <cstdio>
#include <stdexcept>

// 极简串口封装（阻塞读），C++11
class ImuSensor {
public:
    ImuSensor(const std::string& port, int baud) : uart_port(port), baudrate(baud), uart_fd(-1) {}

    bool init() {
        uart_fd = open(uart_port.c_str(), O_RDWR | O_NOCTTY);
        if (uart_fd == -1) {
            perror("打开串口失败");
            return false;
        }

        struct termios options;
        if (tcgetattr(uart_fd, &options) != 0) {
            perror("获取串口属性失败");
            close(uart_fd);
            uart_fd = -1;
            return false;
        }

        speed_t sp = B115200;
        if (baudrate == 9600) sp = B9600;
        else if (baudrate == 19200) sp = B19200;
        else if (baudrate == 38400) sp = B38400;
        else if (baudrate == 57600) sp = B57600;
        else if (baudrate == 115200) sp = B115200;
        else if (baudrate == 230400) sp = B230400;
#ifdef B460800
        else if (baudrate == 460800) sp = B460800;
#endif
#ifdef B921600
        else if (baudrate == 921600) sp = B921600;
#endif

        cfsetispeed(&options, sp);
        cfsetospeed(&options, sp);

        options.c_cflag &= ~CSIZE;
        options.c_cflag |= CS8;       // 8N1
        options.c_cflag &= ~PARENB;
        options.c_cflag &= ~CSTOPB;
        options.c_cflag |= (CLOCAL | CREAD);

        options.c_iflag = 0;
        options.c_oflag = 0;
        options.c_lflag = 0;          // 原始模式
        options.c_cc[VMIN]  = 1;      // 至少1字节
        options.c_cc[VTIME] = 0;      // 无超时（阻塞）

        if (tcsetattr(uart_fd, TCSANOW, &options) != 0) {
            perror("设置串口属性失败");
            close(uart_fd);
            uart_fd = -1;
            return false;
        }

        tcflush(uart_fd, TCIOFLUSH);
        return true;
    }

    ssize_t readSome(uint8_t* buf, size_t cap) {
        if (uart_fd == -1) return -1;
        return read(uart_fd, buf, cap);
    }

    ~ImuSensor() {
        if (uart_fd != -1) {
            close(uart_fd);
            uart_fd = -1;
        }
    }

private:
    std::string uart_port;
    int baudrate;
    int uart_fd;
};

// XBUS 工具函数
static inline uint16_t be16(const uint8_t* p){ return (uint16_t(p[0])<<8)|p[1]; }
static inline uint32_t be32(const uint8_t* p){ return (uint32_t(p[0])<<24)|(uint32_t(p[1])<<16)|(uint32_t(p[2])<<8)|p[3]; }
static inline float befloat32(const uint8_t* p){ uint32_t u = be32(p); float v; std::memcpy(&v, &u, 4); return v; }

static inline uint8_t checksum_basic(uint8_t busId, uint8_t mid, const uint8_t* payload, size_t n, int extLenBytes){
    // 校验和包含 BusID + MID + LEN(或0xFF+LENEX_H+LENEX_L) + PAYLOAD
    uint32_t s = 0;
    s += busId;
    s += mid;
    if (extLenBytes == 0) {
        s += (uint8_t)n;
    } else {
        s += 0xFF;
        s += ((n >> 8) & 0xFF);
        s += (n & 0xFF);
    }
    for (size_t i=0;i<n;++i) s += payload[i];
    return (uint8_t)((0x100 - (s & 0xFF)) & 0xFF);
}

// 遍历 MTData2 的 Packet
template<typename F>
static bool foreachPacket(const std::vector<uint8_t>& pl, F&& cb){
    size_t i = 0, n = pl.size();
    while (i + 3 <= n) {
        uint16_t did = be16(&pl[i]);
        uint8_t sz = pl[i+2];
        i += 3;
        if (i + sz > n) return false; // 截断
        cb(did, &pl[i], sz);
        i += sz;
    }
    return i == n;
}

// XDI 映射与具名打印
namespace XDI {
    static const uint16_t Temperature      = 0x0810;
    static const uint16_t PacketCounter    = 0x1020;
    static const uint16_t SampleTimeCoarse = 0x1070;
    static const uint16_t SampleTimeFine   = 0x1060;
    static const uint16_t Quaternion       = 0x2010;
    static const uint16_t EulerAngles      = 0x2030;
    static const uint16_t Acceleration     = 0x4020;
    static const uint16_t FreeAcceleration = 0x4030;
    static const uint16_t RateOfTurn       = 0x8020;
    static const uint16_t MagneticField    = 0xC020;
    static const uint16_t StatusWord       = 0xE020;
    static const uint16_t DeviceId         = 0xE080;
    static const uint16_t LocationId       = 0xE081;
}

struct FieldMeta { const char* name; const char* unit; };
static inline const FieldMeta& metaOf(uint16_t did){
    static const FieldMeta unknown = {"unknown",""};
    switch (did) {
        case XDI::Acceleration:      { static const FieldMeta m={"加速度","m/s^2"}; return m; }
        case XDI::FreeAcceleration:  { static const FieldMeta m={"自由加速度","m/s^2"}; return m; }
        case XDI::RateOfTurn:        { static const FieldMeta m={"角速度","rad/s"}; return m; }
        case XDI::MagneticField:     { static const FieldMeta m={"磁场","归一化"}; return m; }
        case XDI::EulerAngles:       { static const FieldMeta m={"欧拉角","度"}; return m; }
        case XDI::Quaternion:        { static const FieldMeta m={"四元数",""}; return m; }
        case XDI::Temperature:       { static const FieldMeta m={"温度","°C"}; return m; }
        case XDI::PacketCounter:     { static const FieldMeta m={"计数器",""}; return m; }
        case XDI::SampleTimeFine:    { static const FieldMeta m={"细时间戳","tick"}; return m; }
        case XDI::SampleTimeCoarse:  { static const FieldMeta m={"粗时间戳","秒"}; return m; }
        case XDI::StatusWord:        { static const FieldMeta m={"状态字",""}; return m; }
        case XDI::DeviceId:          { static const FieldMeta m={"设备ID",""}; return m; }
        case XDI::LocationId:        { static const FieldMeta m={"位置ID",""}; return m; }
        default: return unknown;
    }
}

// 帧级聚合与必出项打印
struct FrameData {
    bool has_counter=false; uint32_t counter=0;
    bool has_tfine=false; uint32_t t_fine=0;
    bool has_tcoarse=false; uint32_t t_coarse=0;
    bool has_quat=false; float qw=0, qx=0, qy=0, qz=0;
    bool has_euler=false; float roll=0, pitch=0, yaw=0;
    bool has_acc=false; float ax=0, ay=0, az=0;
    bool has_facc=false; float fax=0, fay=0, faz=0;
    bool has_gyr=false; float gx=0, gy=0, gz=0;
    bool has_status=false; uint32_t status=0;
    void reset(){ *this = FrameData(); }
    void print() const {
        // 时间（组合秒）
        if (has_tcoarse || has_tfine) {
            double t = (has_tcoarse? (double)t_coarse : 0.0) + (has_tfine? ((t_fine % 10000) / 10000.0) : 0.0);
            std::printf("时间 = %.4f 秒\n", t);
        }
        if (has_counter) std::printf("计数器 = %u\n", (unsigned)counter);
        if (has_quat) std::printf("四元数 [w,x,y,z] = [%.6f %.6f %.6f %.6f]\n", qw,qx,qy,qz); else std::printf("四元数 = [无]\n");
        if (has_euler) std::printf("欧拉角 [roll,pitch,yaw] = [%.3f %.3f %.3f] 度\n", roll,pitch,yaw); else std::printf("欧拉角 = [无]\n");
        if (has_acc) std::printf("加速度 [x,y,z] = [%.6f %.6f %.6f] m/s^2\n", ax,ay,az); else std::printf("加速度 = [无]\n");
        if (has_facc) std::printf("自由加速度 [x,y,z] = [%.6f %.6f %.6f] m/s^2\n", fax,fay,faz); else std::printf("自由加速度 = [无]\n");
        if (has_gyr) std::printf("角速度 [x,y,z] = [%.6f %.6f %.6f] rad/s\n", gx,gy,gz); else std::printf("角速度 = [无]\n");
        if (has_status) std::printf("状态字 = 0x%08X\n", (unsigned)status);
        std::printf("------------------------------\n");
    }
};

int main(int argc, char** argv) {
    try {
        std::string dev = "/dev/ttyS0";
        int baud = 115200;
        if (argc >= 2) dev = argv[1];
        if (argc >= 3) baud = std::atoi(argv[2]);

        ImuSensor sensor(dev, baud);
        if (!sensor.init()) {
            std::cerr << "串口初始化失败: " << dev << std::endl;
            return EXIT_FAILURE;
        }

        std::cout << "读取设备: " << dev << ", 波特率: " << baud << std::endl;
        std::cout << "开始接收并解码（Ctrl+C 退出）..." << std::endl;

        std::vector<uint8_t> stream;
        stream.reserve(8192);
        std::vector<uint8_t> payload;
        bool verbose = true;          // 诊断输出
        const size_t rawPreview = 64; // 每次读取预览的原始字节数量上限

        for (;;) {
            uint8_t tmp[512];
            ssize_t n = sensor.readSome(tmp, sizeof(tmp));
            if (n < 0) {
                perror("读取数据失败");
                return EXIT_FAILURE;
            } else if (n == 0) {
                usleep(10000);
                continue;
            }
            if (verbose) {
                std::cout << "read " << n << " bytes: ";
                size_t preview = n < (ssize_t)rawPreview ? (size_t)n : rawPreview;
                for (size_t k=0;k<preview;++k) std::printf("%02X ", tmp[k]);
                if ((size_t)n > preview) std::cout << "...";
                std::cout << std::endl;
            }
            stream.insert(stream.end(), tmp, tmp + n);

            // 解析 XBUS 帧（可能一次读出多帧）
            size_t i = 0;
            while (i < stream.size()) {
                // 寻找 PRE=0xFA
                while (i < stream.size() && stream[i] != 0xFA) ++i;
                if (i + 3 >= stream.size()) break; // 需要 BusID, MID, LEN
                size_t start = i;
                uint8_t pre = stream[i++];
                (void)pre;
                uint8_t busId = stream[i++];
                uint8_t mid = stream[i++];
                uint8_t len = stream[i++];

                size_t need = 0;
                int extBytes = 0;
                if (len == 0xFF) {
                    if (i + 2 > stream.size()) { i = start + 1; break; }
                    extBytes = 2;
                    need = (size_t(stream[i]) << 8) | stream[i+1];
                    i += 2;
                } else {
                    need = len;
                }

                if (need > (1<<16)) { // 长度过大，丢弃该起点，继续找下一个 0xFA
                    i = start + 1;
                    continue;
                }

                if (i + need + 1 > stream.size()) { // payload + CS 未齐
                    // 回退到起始，等待更多数据
                    i = start; 
                    break;
                }

                const uint8_t* pl = &stream[i];
                uint8_t cs = stream[i + need];
                uint8_t calc = checksum_basic(busId, mid, pl, need, extBytes);

                if (cs != calc) {
                    // 校验失败，丢弃一个字节继续
                    if (verbose) {
                        std::fprintf(stderr, "checksum fail bus=0x%02X mid=0x%02X need=%zu got_cs=0x%02X calc=0x%02X\n", busId, mid, need, cs, calc);
                    }
                    i = start + 1;
                    continue;
                }

                payload.assign(pl, pl + need);
                i += need + 1; // 跳过 payload 与 CS

                // 仅处理 MTData2，其它 MID 做最小提示
                if (mid == 0x36) {
                    if (verbose) std::cout << "MTData2 frame payload=" << need << " bytes" << std::endl;
                    FrameData fd;
                    bool ok = foreachPacket(payload, [&](uint16_t did, const uint8_t* p, uint8_t sz){
                        if (did == XDI::PacketCounter && (sz==2 || sz==4)) { fd.has_counter=true; fd.counter = (sz==2)? be16(p) : be32(p); }
                        else if (did == XDI::SampleTimeFine && sz==4) { fd.has_tfine=true; fd.t_fine = be32(p); }
                        else if (did == XDI::SampleTimeCoarse && sz==4) { fd.has_tcoarse=true; fd.t_coarse = be32(p); }
                        else if (did == XDI::Quaternion && sz==16) { fd.has_quat=true; fd.qw = befloat32(p); fd.qx = befloat32(p+4); fd.qy = befloat32(p+8); fd.qz = befloat32(p+12); }
                        else if (did == XDI::EulerAngles && sz==12) { fd.has_euler=true; fd.roll = befloat32(p); fd.pitch = befloat32(p+4); fd.yaw = befloat32(p+8); }
                        else if (did == XDI::Acceleration && sz==12) {
                            // 轴顺序为 [x, y, z]
                            fd.has_acc=true; fd.ax = befloat32(p); fd.ay = befloat32(p+4); fd.az = befloat32(p+8);
                        }
                        else if (did == XDI::FreeAcceleration && sz==12) {
                            fd.has_facc=true; fd.fax = befloat32(p); fd.fay = befloat32(p+4); fd.faz = befloat32(p+8);
                        }
                        else if (did == XDI::RateOfTurn && sz==12) { fd.has_gyr=true; fd.gx = befloat32(p); fd.gy = befloat32(p+4); fd.gz = befloat32(p+8); }
                        else if (did == XDI::StatusWord && sz==4) { fd.has_status=true; fd.status = be32(p); }
                        // 其他忽略
                    });
                    if (!ok) {
                        std::cerr << "MTData2 包截断，丢弃本帧" << std::endl;
                    } else {
                        fd.print();
                    }
                } else if (verbose) {
                    std::cout << "ignore MID=0x" << std::hex << (int)mid << std::dec << ", payload=" << need << " bytes" << std::endl;
                }
            }

            // 清理已消费数据
            if (i > 0) {
                stream.erase(stream.begin(), stream.begin() + i);
            }
        }

    } catch (const std::exception& e) {
        std::cerr << "错误: " << e.what() << std::endl;
        return EXIT_FAILURE;
    }

    return EXIT_SUCCESS;
}